/*
 * motor_drv.c
 *
 *  Created on: Oct 12, 2024
 *      Author: Jinming
 */
#include <string.h>
#include <math.h>
#include "motor_drv.h"
#include "delay_us.h"

#define TAG	"motor"

const osMutexAttr_t g_tMotorMutexAttr = {
  .name = "Motor"
};

static int setMotorInfo(MotorUnit_t *motor, uint8_t *pData, uint8_t len)
{
	CANMsgRx_t frame;
	osStatus_t osStatus;

	osStatus = osMutexAcquire(motor->mutex, 10);
	if(osStatus)
	{
		return osStatus;
	}

	while(motor->receive(&frame, 0) == 0)			//有接收的数据未处理
	{
		if(frame.id == ((motor->canId & 0x7F) + 0x80))		//错误码
		{
			motor->errCode = frame.data[1];
			motor->errCode <<= 8;
			motor->errCode |= frame.data[0];
		}
		else if(frame.id == ((motor->canId & 0x7F) + 0x700))
		{
			motor->errCode = 0;
		}
	}

	motor->send(motor->canId, pData, len);
	motor->clear();
	if(motor->receive(&frame, 10))
	{
		LOGE(TAG, "set motor info error");
	}
	else
	{
		if(frame.id == ((motor->canId & 0x7F) + 0x80))		//错误码
		{
			motor->errCode = frame.data[1];
			motor->errCode <<= 8;
			motor->errCode |= frame.data[0];
		}
		else if(frame.id == ((motor->canId & 0x7F) + 0x700))  //上电初始化
		{
			motor->errCode = 0;
		}
	}
	osMutexRelease(motor->mutex);

    if((frame.id & 0x7F) == (motor->canId & 0x7F))
    {
        if(frame.data[0] == 0x60)
        {
            return 0;
        }
        else if(frame.data[0] == 0x80)
        {
            return -1;
        }
        else
        {
            return -1;
        }
    }

    return -1;
}

static int getMotorInfo(MotorUnit_t *motor, uint8_t *pData, uint8_t len, int *output)
{
    int ret = 0;
	CANMsgRx_t frame;
	osStatus_t osStatus;

	osStatus = osMutexAcquire(motor->mutex, 10);
	if(osStatus)
	{
		return osStatus;
	}

	while(motor->receive(&frame, 0) == 0)			//有接收的数据未处理
	{
		if(frame.id == ((motor->canId & 0x7F) + 0x80))		//错误码
		{
			motor->errCode = frame.data[1];
			motor->errCode <<= 8;
			motor->errCode |= frame.data[0];
		}
		else if(frame.id == ((motor->canId & 0x7F) + 0x700))	//上电初始化
		{
			motor->errCode = 0;
		}
	}


	motor->send(motor->canId, pData, len);
	motor->clear();
	if(motor->receive(&frame, 10))
	{
		output = 0;
		LOGE(TAG, "get motor info error");
		osMutexRelease(motor->mutex);
		return -1;
	}
	else
	{
		if(frame.id == ((motor->canId & 0x7F) + 0x80))		//错误码
		{
			motor->errCode = frame.data[1];
			motor->errCode <<= 8;
			motor->errCode |= frame.data[0];
		}
		else if(frame.id == ((motor->canId & 0x7F) + 0x700))  //上电初始化
		{
			motor->errCode = 0;
		}
	}
	osMutexRelease(motor->mutex);

    if((frame.id & 0x7F) == (motor->canId & 0x7F))
    {
        if((frame.data[0] & 0xF) == 0x3)       //4字节
        {
            ret = frame.data[7];
            ret <<= 8;
            ret |= frame.data[6];
            ret <<= 8;
            ret |= frame.data[5];
            ret <<= 8;
            ret |= frame.data[4];

            *output = ret;
            return 0;
        }
        else if((frame.data[0] & 0xF) == 0x7)      //3字节
        {
            ret = frame.data[6];
            ret <<= 8;
            ret |= frame.data[5];
            ret <<= 8;
            ret |= frame.data[4];

            if(frame.data[6] >= 0x80)
            {
                ret |= 0xFF000000;
            }

            *output = ret;
            return 0;
        }
        else if((frame.data[0] & 0xF) == 0xB)      //2字节
        {
            ret = frame.data[5];
            ret <<= 8;
            ret |= frame.data[4];

            if(frame.data[5] >= 0x80)
            {
                ret |= 0xFFFF0000;
            }

            *output = ret;
            return 0;
        }
        else if((frame.data[0] & 0xF) == 0xF)
        {
            *output = frame.data[4];
            if(frame.data[4] >= 0x80)
            {
                *output |= 0xFFFFFF00;
            }
            return 0;
        }
        else
        {
            return -1;
        }
    }

    return -1;
}

int motor_state_get(MotorUnit_t *motor, CIA402_STATE_e *state)
{
	int temp, ret;
	uint8_t value, HighHalfByte;

	uint8_t aStateWord[] = {0x40, 0x41, 0x60, 0, 0, 0, 0, 0};

	ret = getMotorInfo(motor, aStateWord, sizeof(aStateWord), &temp);
	if(ret)
	{
		return -1;
	}

    HighHalfByte = (temp >> 5) & 0x3;
    value = (HighHalfByte << 4) | (temp & 0xF);
    if((value == 0) || (value == 0x10))
    {
        *state = CIA_NOT_READY_SWITCH_ON;
    }
    else if((value == 0x20) || (value == 0x30))
    {
    	*state = CIA_SWITCH_ON_DISABLE;
    }
    else if(value == 0x11)
    {
    	*state = CIA_READY_SWITCH_ON;
    }
    else if(value == 0x13)
    {
    	*state = CIA_SWITCH_ON;
    }
    else if(value == 0x17)
    {
    	*state = CIA_OPERATION_ENABLE;
    }
    else if(value == 0x07)
    {
    	*state = CIA_QUCIK_STOP;
    }
    else if((value == 0xF) || (value == 0x1F))
    {
    	*state = CIA_FAULT_REACTION;
    }
    else if((value == 0x08) || (value == 0x18))
    {
    	*state = CIA_FAULT;
    }

    return 0;
}

int motor_state_change(MotorUnit_t *motor, CIA402_STATE_e curState, CIA402_STATE_e targetState)
{
	uint8_t aReadControlWord[] = {0x40, 0x40, 0x60, 0, 0, 0, 0, 0};
	uint8_t aControlWord[] = {0x2B, 0x40, 0x60, 0, 0, 0, 0, 0};
    unsigned short hControlWord = 0xFF;
    int temp;

    for(int i = 0; i < 20; i++)
    {
        if(curState == targetState)
            return 0;

        switch(curState)
        {
        case CIA_NOT_READY_SWITCH_ON:       //该状态会自动跳转，等待即可
            break;

        case CIA_SWITCH_ON_DISABLE:
            if(targetState == CIA_NOT_READY_SWITCH_ON)      //无法往上跳转，没有跳转路径
                return -1;

            switch(targetState)
            {
            case CIA_READY_SWITCH_ON:
            case CIA_SWITCH_ON:
            case CIA_OPERATION_ENABLE:
            case CIA_QUCIK_STOP:
                aControlWord[4] = 0x6;
                break;

            default:                //其他目标状态不允许跳转
                return -1;
                break;
            }
            break;

        case CIA_READY_SWITCH_ON:
            switch(targetState)
            {
            case CIA_SWITCH_ON:
            case CIA_OPERATION_ENABLE:
            case CIA_QUCIK_STOP:
                aControlWord[4] = 0x7;
                break;

            case CIA_SWITCH_ON_DISABLE:
                aControlWord[4] = 0x2;
                break;

            default:                //其他目标状态不允许跳转
                return -1;
                break;
            }
            break;

        case CIA_SWITCH_ON:
            switch(targetState)
            {
            case CIA_OPERATION_ENABLE:
            case CIA_QUCIK_STOP:
                aControlWord[4] = 0xF;
                break;

            case CIA_SWITCH_ON_DISABLE:
                aControlWord[4] = 0x2;
                break;

            case CIA_READY_SWITCH_ON:
                aControlWord[4] = 0x6;
                break;

            default:                //其他目标状态不允许跳转
                return -1;
                break;
            }
            break;

        case CIA_OPERATION_ENABLE:
            switch(targetState)
            {
            case CIA_SWITCH_ON_DISABLE:
                aControlWord[4] = 0x0;
                break;

            case CIA_READY_SWITCH_ON:
                aControlWord[4] = 0x6;
                break;

            case CIA_SWITCH_ON:
                aControlWord[4] = 0x7;
                break;

            case CIA_QUCIK_STOP:
                aControlWord[4] = 0x3;
                break;

            default:                //其他目标状态不允许跳转
                return -1;
                break;
            }
            break;

        case CIA_QUCIK_STOP:
            if(targetState == CIA_SWITCH_ON_DISABLE)
            {
                aControlWord[4] = 0x0;
            }
            else        //其他目标状态不允许跳转
            {
                return -1;
            }
            break;

        case CIA_FAULT:
            if(targetState == CIA_SWITCH_ON_DISABLE)       //Bit7需要发送0-->1
            {
                if(hControlWord == 0xFF)
                {
                    getMotorInfo(motor, aReadControlWord, sizeof(aReadControlWord), &temp);
                    hControlWord = temp;
                    aControlWord[4] = hControlWord & 0xFF7F;
                }
                else
                {
                    getMotorInfo(motor, aReadControlWord, sizeof(aReadControlWord), &temp);
                    hControlWord = temp;
                    aControlWord[4] = hControlWord | 0x80;
                }
            }
            else
            {
                return -1;
            }
            break;

        case CIA_FAULT_REACTION:
            if(targetState == CIA_FAULT)        //等待自动跳转
            {
                ;
            }
            else
            {
                return -1;
            }
            break;

        default:
            break;
        }

        setMotorInfo(motor, aControlWord, sizeof(aControlWord));
        if(((curState == CIA_OPERATION_ENABLE) && (targetState == CIA_QUCIK_STOP)) || \
           ((curState == CIA_OPERATION_ENABLE) && (targetState == CIA_SWITCH_ON_DISABLE)))
        {       //不等待
            LOGE(TAG, "State:%d-->%d\n", curState, targetState);
            return 0;
        }
        else
        {
            delay_us(1000);
            LOGD(TAG, "State:%d-->", curState);
            motor_state_get(motor, &curState);
            LOGD(TAG, "%d\n", curState);
        }
    }

    return -1;
}

//设置加速度，单位RPM/s
int motor_acc_set(MotorUnit_t *motor, float rpmps)
{
	MotorUnit_t *ptr = motor;

    unsigned char aAccCmd[] = {0x23, 0x83, 0x60, 0, 0, 0, 0, 0};
    int acc = rpmps / 60 * ptr->tMotorAttr.encoderResolution * ptr->tMotorAttr.reductionRatio;
    LOGD(TAG, "acc:%d\n", acc);

    aAccCmd[4] = acc & 0xff;
    aAccCmd[5] = (acc >> 8) & 0xff;
    aAccCmd[6] = (acc >> 16) & 0xff;
    aAccCmd[7] = (acc >> 24) & 0xff;
    return setMotorInfo(motor, aAccCmd, sizeof(aAccCmd));
}

//获取电机加转速设置，单位RPM/s
int motor_acc_get(MotorUnit_t *motor, float *acc)
{
	MotorUnit_t *ptr = motor;
    unsigned char aSpeedCmd[] = {0x40, 0x83, 0x60, 0, 0, 0, 0, 0};
    int res, ret;

    ret = getMotorInfo(motor, aSpeedCmd, sizeof(aSpeedCmd), &res);
    if(ret == 0)
    {
        *acc = res * 60 / ptr->tMotorAttr.encoderResolution / ptr->tMotorAttr.reductionRatio;
        // LOGD(TAG, "speed:%d\t %f\n", res, *acc);
    }

    return ret;
}

//设置减速度，单位RPM/s
int motor_dec_set(MotorUnit_t *motor, float rpmps)
{
	MotorUnit_t *ptr = motor;
    unsigned char aDecCmd[] = {0x23, 0x84, 0x60, 0, 0, 0, 0, 0};
    int dec = rpmps / 60 * ptr->tMotorAttr.encoderResolution * ptr->tMotorAttr.reductionRatio;

    aDecCmd[4] = dec & 0xff;
    aDecCmd[5] = (dec >> 8) & 0xff;
    aDecCmd[6] = (dec >> 16) & 0xff;
    aDecCmd[7] = (dec >> 24) & 0xff;
    return setMotorInfo(motor, aDecCmd, sizeof(aDecCmd));
}

//获取电机减转速设置，单位RPM/s
int motor_dec_get(MotorUnit_t *motor, float *Dec)
{
    unsigned char aSpeedCmd[] = {0x40, 0x84, 0x60, 0, 0, 0, 0, 0};
    int res, ret;

    ret = getMotorInfo(motor, aSpeedCmd, sizeof(aSpeedCmd), &res);
    if(ret == 0)
    {
        *Dec = res * 60 / motor->tMotorAttr.encoderResolution / motor->tMotorAttr.reductionRatio;
        // printf("speed:%d\t %f\n", res, *Dec);
    }

    return ret;
}

//设置快停减速度，单位RPM/s
int motor_quick_stop_dec_set(MotorUnit_t *motor, float rpmps)
{
    unsigned char aQuickStopDec[] = {0x23, 0x85, 0x60, 0, 0, 0, 0, 0};
    int dec = rpmps / 60 * motor->tMotorAttr.encoderResolution * motor->tMotorAttr.reductionRatio;

    aQuickStopDec[4] = dec & 0xff;
    aQuickStopDec[5] = (dec >> 8) & 0xff;
    aQuickStopDec[6] = (dec >> 16) & 0xff;
    aQuickStopDec[7] = (dec >> 24) & 0xff;
    return setMotorInfo(motor, aQuickStopDec, sizeof(aQuickStopDec));
}

//获取电机快停减转速设置，单位RPM/s
int motor_quick_stop_dec_get(MotorUnit_t *motor, float *Dec)
{
    unsigned char aQuickStopDec[] = {0x40, 0x85, 0x60, 0, 0, 0, 0, 0};
    int res, ret;

    ret = getMotorInfo(motor, aQuickStopDec, sizeof(aQuickStopDec), &res);
    if(ret == 0)
    {
        *Dec = res * 60 / motor->tMotorAttr.encoderResolution * motor->tMotorAttr.reductionRatio;
        // printf("speed:%d\t %f\n", res, *Dec);
    }

    return ret;
}

//设置急加度，单位RPM/s^s
int motor_jerk_set(MotorUnit_t *motor, float rpmpss)
{
    unsigned char aJerkCmd[] = {0x23, 0xA4, 0x60, 0x1, 0, 0, 0, 0};
    int jerk = rpmpss / 60 * motor->tMotorAttr.encoderResolution * motor->tMotorAttr.reductionRatio;

    aJerkCmd[4] = jerk & 0xff;
    aJerkCmd[5] = (jerk >> 8) & 0xff;
    aJerkCmd[6] = (jerk >> 16) & 0xff;
    aJerkCmd[7] = (jerk >> 24) & 0xff;
    return setMotorInfo(motor, aJerkCmd, sizeof(aJerkCmd));
}

//获取急加度，单位RPM/s^s
int motor_jerk_get(MotorUnit_t *motor, float *jerk)
{
    unsigned char aJerkCmd[] = {0x40, 0x84, 0x60, 0x1, 0, 0, 0, 0};
    int res, ret;

    ret = getMotorInfo(motor, aJerkCmd, sizeof(aJerkCmd), &res);
    if(ret == 0)
    {
        *jerk = res * 60 / motor->tMotorAttr.encoderResolution / motor->tMotorAttr.reductionRatio;
        // printf("speed:%d\t %f\n", res, *Dec);
    }

    return ret;
}

//速度模式下设置目标速度，单位RPM
int motor_speed_set(MotorUnit_t *motor, float rpm)
{
    unsigned char aSpeedCmd[] = {0x23, 0xFF, 0x60, 0, 0, 0, 0, 0};
    int speed = rpm * motor->tMotorAttr.encoderResolution * motor->tMotorAttr.reductionRatio / 60;

    aSpeedCmd[4] = speed & 0xff;
    aSpeedCmd[5] = (speed >> 8) & 0xff;
    aSpeedCmd[6] = (speed >> 16) & 0xff;
    aSpeedCmd[7] = (speed >> 24) & 0xff;
    return setMotorInfo(motor, aSpeedCmd, sizeof(aSpeedCmd));
}

//获取电机实际转速，单位RPM
int motor_speed_get(MotorUnit_t *motor, float *speed)
{
    unsigned char aSpeedCmd[] = {0x40, 0x6C, 0x60, 0, 0, 0, 0, 0};
    int res, ret;

    ret = getMotorInfo(motor, aSpeedCmd, sizeof(aSpeedCmd), &res);
    if(ret == 0)
    {
        *speed = res * 60 / motor->tMotorAttr.encoderResolution / motor->tMotorAttr.reductionRatio;
        // printf("speed: %f\n", *speed);
    }

    return ret;
}

//获取电机最大转速，单位RPM
int motor_maxspeed_get(MotorUnit_t *motor, float *maxSpeed)
{
    unsigned char aSpeedCmd[] = {0x40, 0x7F, 0x60, 0, 0, 0, 0, 0};
    int res, ret;

    ret = getMotorInfo(motor, aSpeedCmd, sizeof(aSpeedCmd), &res);
    if(ret == 0)
    {
        *maxSpeed = res * 60 / motor->tMotorAttr.encoderResolution / motor->tMotorAttr.reductionRatio;
        // printf("max speed:%d\n", res);
    }

    return ret;
}

//获取电机最大转速，单位RPM
int motor_maxspeed_set(MotorUnit_t *motor, float maxSpeed)
{
    unsigned char aSpeedCmd[] = {0x23, 0x7F, 0x60, 0, 0, 0, 0, 0};
    int speed = maxSpeed * motor->tMotorAttr.encoderResolution * motor->tMotorAttr.reductionRatio / 60;

    aSpeedCmd[4] = speed & 0xff;
    aSpeedCmd[5] = (speed >> 8) & 0xff;
    aSpeedCmd[6] = (speed >> 16) & 0xff;
    aSpeedCmd[7] = (speed >> 24) & 0xff;
    return setMotorInfo(motor, aSpeedCmd, sizeof(aSpeedCmd));
}

//设置电机的目标位置，单位：度
int motor_position_set(MotorUnit_t *motor, float pos)
{
    unsigned char aPositionCmd[] = {0x23, 0x7A, 0x60, 0, 0, 0, 0, 0};
    int temp = pos / 360 * motor->tMotorAttr.encoderResolution;

    aPositionCmd[4] = temp & 0xff;
    aPositionCmd[5] = (temp >> 8) & 0xff;
    aPositionCmd[6] = (temp >> 16) & 0xff;
    aPositionCmd[7] = (temp >> 24) & 0xff;
    return setMotorInfo(motor, aPositionCmd, sizeof(aPositionCmd));
}

//获取电机当前位置，单位：度
int motor_position_get(MotorUnit_t *motor, float *pos)
{
    int res, ret;
    unsigned char aPositionCmd[] = {0x40, 0x64, 0x60, 0, 0, 0, 0, 0};

    res = getMotorInfo(motor, aPositionCmd, sizeof(aPositionCmd), &ret);
    if(res == 0)
    {
        *pos = ret / (motor->tMotorAttr.encoderResolution * 1.0f) * 360;
		*pos = *pos / motor->tMotorAttr.reductionRatio;
    }

    return res;
}

//设置电机的力矩，单位：Nm
int motor_torque_set(MotorUnit_t *motor, float Nm)
{
    unsigned char aTorqueCmd[] = {0x2B, 0x71, 0x60, 0, 0, 0, 0, 0};
    int16_t temp = Nm / motor->tMotorAttr.ratedTorque * 1000;

    aTorqueCmd[4] = temp & 0xff;
    aTorqueCmd[5] = (temp >> 8) & 0xff;
//    aTorqueCmd[6] = (temp >> 16) & 0xff;
//    aTorqueCmd[7] = (temp >> 24) & 0xff;
    return setMotorInfo(motor, aTorqueCmd, sizeof(aTorqueCmd));
}

//获取电机当前力矩，单位：Nm
int motor_torque_get(MotorUnit_t *motor, float *Nm)
{
    int res, ret;
    unsigned char aTorqueCmd[] = {0x40, 0x77, 0x60, 0, 0, 0, 0, 0};

    res = getMotorInfo(motor, aTorqueCmd, sizeof(aTorqueCmd), &ret);
    if(res == 0)
    {
        *Nm = ret * motor->tMotorAttr.ratedTorque / 1000;
        // printf("torque:%d\n", ret);
    }

    return res;
}

//获取电机当前电流，单位：A
int motor_current_get(MotorUnit_t *motor, float *ampere)
{
    int res, ret;
    unsigned char aCurrentCmd[] = {0x40, 0x78, 0x60, 0, 0, 0, 0, 0};

    res = getMotorInfo(motor, aCurrentCmd, sizeof(aCurrentCmd), &ret);
    if(res == 0)
    {
        *ampere = ret * motor->tMotorAttr.ratedCurrent / 1000;
    }

    return res;
}

//设置电机的力矩，单位：Nm
int motor_torque_acc_set(MotorUnit_t *motor, float NmPerSec)
{
    unsigned char aTorqueCmd[] = {0x23, 0x87, 0x60, 0, 0, 0, 0, 0};
    uint32_t temp = NmPerSec / motor->tMotorAttr.ratedTorque * 1000;

    aTorqueCmd[4] = temp & 0xff;
    aTorqueCmd[5] = (temp >> 8) & 0xff;
    aTorqueCmd[6] = (temp >> 16) & 0xff;
    aTorqueCmd[7] = (temp >> 24) & 0xff;
    return setMotorInfo(motor, aTorqueCmd, sizeof(aTorqueCmd));
}

int motor_quickStop(MotorUnit_t *motor)
{
    unsigned char aStateWord[] = {0x40, 0x41, 0x60, 0, 0, 0, 0, 0};
    int temp;

    getMotorInfo(motor, aStateWord, sizeof(aStateWord), &temp);
    motor_state_get(motor, &motor->curState);
    LOGD(TAG, "motor state:%x\n", motor->curState);
    int ret = motor_state_change(motor, motor->curState, CIA_QUCIK_STOP);     //先把状态切换到快停模式
    if(ret != 0)
    {
        LOGE(TAG, "Motor state change error\n");
        return -1;
    }

    return 0;
}

int motor_Idle(MotorUnit_t *motor)
{
    unsigned char aStateWord[] = {0x40, 0x41, 0x60, 0, 0, 0, 0, 0};
    int temp;

    getMotorInfo(motor, aStateWord, sizeof(aStateWord), &temp);
    motor_state_get(motor, &motor->curState);
    if(motor->curState == CIA_READY_SWITCH_ON)
    	return 0;
    LOGD(TAG, "motor state:%x\n", motor->curState);
    int ret = motor_state_change(motor, motor->curState, CIA_READY_SWITCH_ON);     //先把状态切换到快停模式
    if(ret != 0)
    {
        LOGE(TAG, "Motor state change error\n");
        return -1;
    }

    return 0;
}

int motor_mode_get(MotorUnit_t *motor, MOTOR_MODE_e *mode)
{
	int res, ret;
    unsigned char modeWord[] = {0x40, 0x61, 0x60, 0, 0, 0, 0, 0};

    res = getMotorInfo(motor, modeWord, sizeof(modeWord), &ret);
    if(res == 0)
    {
    	*mode = ret;
    }

    return res;
}

int motor_enter_position_mode(MotorUnit_t *motor)
{
    uint8_t aMotionMode[] = {0x2F, 0x60, 0x60, 0, 1, 0, 0, 0};
    uint8_t aControlWord[] = {0x2B, 0x40, 0x60, 0, 0, 0, 0, 0};

    motor_state_get(motor, &motor->curState);
    LOGD(TAG, "motor state:%x\n", motor->curState);
    int ret = motor_state_change(motor, motor->curState, CIA_READY_SWITCH_ON);     //先把状态切换到可配置状态
    if(ret != 0)
    {
        LOGE(TAG, "Motor state change error\n");
        return -1;
    }

    motor_maxspeed_set(motor, motor->tMotorAttr.maxSpeed);
    motor_acc_set(motor, motor->tMotorAttr.maxAcc);
    motor_dec_set(motor, motor->tMotorAttr.maxDec);
    motor_quick_stop_dec_set(motor, motor->tMotorAttr.stopDec);

    setMotorInfo(motor, aMotionMode, sizeof(aMotionMode));

    aControlWord[4] = 0x2F;			//绝对位置模式
    setMotorInfo(motor, aControlWord, sizeof(aControlWord));

    aControlWord[4] = 0x3F;			//立即生效位置
    setMotorInfo(motor, aControlWord, sizeof(aControlWord));

    ret = motor_state_change(motor, motor->curState, CIA_OPERATION_ENABLE);     //先把状态切换到可配置状态
    if(ret != 0)
    {
        LOGE(TAG, "Motor state change error\n");
        return -1;
    }

    return 0;
}

int motor_enter_speed_mode(MotorUnit_t *motor)
{
    uint8_t aMotionMode[] = {0x2F, 0x60, 0x60, 0, 3, 0, 0, 0};
    uint8_t aspeed[] = {0x23, 0xFF, 0x60, 0, 50, 0, 0, 0};

    motor_state_get(motor, &motor->curState);
    LOGD(TAG, "motor state:%x\n", motor->curState);
    int ret = motor_state_change(motor, motor->curState, CIA_READY_SWITCH_ON);     //先把状态切换到可配置状态
    if(ret != 0)
    {
        LOGE(TAG, "Motor state change error\n");
        return -1;
    }

    motor_maxspeed_set(motor, motor->tMotorAttr.maxSpeed);
    motor_acc_set(motor, motor->tMotorAttr.maxAcc);
    motor_dec_set(motor, motor->tMotorAttr.maxDec);
    motor_quick_stop_dec_set(motor, motor->tMotorAttr.stopDec);

    setMotorInfo(motor, aMotionMode, sizeof(aMotionMode));
    setMotorInfo(motor, aspeed, sizeof(aspeed));

    ret = motor_state_change(motor, motor->curState, CIA_OPERATION_ENABLE);     //先把状态切换到可配置状态
    if(ret != 0)
    {
        LOGE(TAG, "Motor state change error\n");
        return -1;
    }

    return 0;
}

int motor_enter_torque_mode(MotorUnit_t *motor)
{
    uint8_t aMotionMode[] = {0x2F, 0x60, 0x60, 0, 4, 0, 0, 0};

    motor_state_get(motor, &motor->curState);
    LOGD(TAG, "motor state:%x\n", motor->curState);
    int ret = motor_state_change(motor, motor->curState, CIA_READY_SWITCH_ON);     //先把状态切换到可配置状态
    if(ret != 0)
    {
        LOGE(TAG, "Motor state change error\n");
        return -1;
    }

    setMotorInfo(motor, aMotionMode, sizeof(aMotionMode));
    motor_torque_acc_set(motor, motor->tMotorAttr.tffAccDec);
    motor_torque_set(motor, 0);

    ret = motor_state_change(motor, motor->curState, CIA_OPERATION_ENABLE);     //先把状态切换到可配置状态
    if(ret != 0)
    {
        LOGE(TAG, "Motor state change error\n");
        return -1;
    }

    return 0;
}

int motor_mode_change(MotorUnit_t *motor, uint16_t mode)
{
//    MOTOR_MODE_e eCurMode;
    int ret;

//    motor_mode_get(motor, &eCurMode);
//    LOGD(TAG, "cur mode: %d\n", eCurMode);
    if(motor->curRunMode == mode)			//已经处于该模式了，不需要切换控制模式
    {
    	return 0;
    }

    if(mode == MOTOR_MODE_POS)
    {
    	ret = motor_enter_position_mode(motor);
    	if(ret == 0)
    	{
    		motor->curRunMode = MOTOR_MODE_POS;
    		LOGD(TAG, "motor %lx change to %d mode", motor->canId, motor->curRunMode);
    	}
    }
    else if(mode == MOTOR_MODE_RPM)
    {
    	ret = motor_enter_speed_mode(motor);
    	if(ret == 0)
		{
			motor->curRunMode = MOTOR_MODE_RPM;
			LOGD(TAG, "motor %lx change to %d mode", motor->canId, motor->curRunMode);
		}
    }
    else if(mode == MOTOR_MODE_TORQUE)
    {
    	ret = motor_enter_torque_mode(motor);
    	if(ret == 0)
		{
			motor->curRunMode = MOTOR_MODE_TORQUE;
			LOGD(TAG, "motor %lx change to %d mode", motor->canId, motor->curRunMode);
		}
    }
    else if(mode == MOTOR_MODE_IDLE)
    {
    	ret = motor_Idle(motor);
    	if(ret == 0)
    	{
    		motor->curRunMode = MOTOR_MODE_IDLE;
    		LOGD(TAG, "motor %lx change to %d mode", motor->canId, motor->curRunMode);
    	}
    }

    return 0;
}

MOTOR_MODE_e motor_curMode_get(MotorUnit_t *motor)
{
	return motor->curRunMode;
}

int motor_init(MotorUnit_t *motor, uint32_t id, MotorAttr_t *attr)
{
	motor->canId = id;
    memcpy(&motor->tMotorAttr, attr, sizeof(MotorAttr_t));
	motor->mutex = osMutexNew(&g_tMotorMutexAttr);
	if(motor->mutex == NULL)
	{
		LOGE(TAG, "Motor %ld mutex create error", id);
		return -1;
	}

    return 0;
}

void motor_exit(MotorUnit_t *motor)
{
    unsigned char aControlWord[] = {0x2B, 0x40, 0x60, 0, 6, 0, 0, 0};
    setMotorInfo(motor, aControlWord, sizeof(aControlWord));
    osMutexDelete(motor->mutex);
}

int motor_test(MotorUnit_t *motor)
{
    float speed = 0;
    static int cnt = 0;
    float curSpeed;
    static float prePos = 0.0f;
    static float curPos = 0.0f;
    static float period = 1.0f;        //s  正弦波周期
    static unsigned int cycle = 10000;        //us 执行时间间隔
//    float pos;

//    unsigned char aControlWord[] = {0x2B, 0x40, 0x60, 0, 0, 0, 0, 0};

    curPos = 270 * sin(2 * 3.141592654f * cnt / period / (1000000.0f / cycle));
    speed = (curPos - prePos) * (1000000 / cycle) / 6;
    motor_speed_set(motor, speed);
    prePos = curPos;

    LOGD(TAG, "target Speed:%f  ", speed);
    // usleep(cycle);
    motor_speed_get(motor, &curSpeed);
    // getPosition(&pos);
    // getTorque(&pos);

    // cnt++;
    // if(cnt > 1000)
    // {
    //     aControlWord[4] = 6;
    //     setMotorInfo(canId, aControlWord, sizeof(aControlWord));
    //     return -1;
    // }
    return 0;
}




